/***************************************************************************************************************************
* drone_pos_controller.cpp
*
* Author: SFL
*
* Update Time: 2020.9.9
*
* Introduction:  Fast Drone Position Controller 
*         1. 从应用层节点订阅/fast_drone/control_command话题（ControlCommand.msg），接收来自上层的控制指令。
*         2. 从pos_estimator读取无人机的状态信息（DroneState.msg）。
*         3. 调用位置环控制算法，计算加速度控制量。可选择cascade_PID, PID, UDE, passivity-UDE, NE+UDE位置控制算法。目前使用串级pid
*         4. 通过高频头将控制指令发到接收机，具体的接收机部分代码由YSC封装
*         5. 发送相关信息至地面站节点(/fast_drone_pkg/groud_log)，供监控使用。
***************************************************************************************************************************/



#include <ros/ros.h>
#include <Eigen/Eigen>

#include <fast_drone_pkg/AttitudeReference.h>
#include <fast_drone_pkg/ControlCommand.h>
#include <fast_drone_pkg/DroneState.h>
#include <fast_drone_pkg/Topic_for_log.h>
#include <pos_controller_cascade_PID.h>
#include <fast_drone_pkg/ControlOutput.h>
#include <command_to_drone.h>
#include <circle_trajectory.h>
#include <eight_trajectory.h>
#include <usart_to_sbus.h>
#include <drone_utils.h>


using namespace std;


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
fast_drone_pkg::ControlCommand Command_Now;                      //无人机当前执行命令
fast_drone_pkg::ControlCommand Command_Last;                     //无人机上一条执行命令
fast_drone_pkg::ControlCommand Command_to_gs;                    //发送至地面站的指令
fast_drone_pkg::DroneState _DroneState;                          //无人机状态量
Eigen::Vector3d throttle_sp;
fast_drone_pkg::ControlOutput _ControlOutput;
fast_drone_pkg::AttitudeReference _AttitudeReference;           //位置控制器输出，即姿态环参考量
fast_drone_pkg::Topic_for_log _Topic_for_log;                  //用于日志记录的topic
float cur_time;

float Takeoff_height;                                       //起飞高度
float Disarm_height;                                        //自动上锁高度

//Geigraphical fence 地理围栏
Eigen::Vector2f geo_fence_x;
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;
//起飞的位置
Eigen::Vector3d Takeoff_position = Eigen::Vector3d(0.0,0.0,0.0);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param();
int check_failsafe();

void drone_state_cb(const fast_drone_pkg::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;

    _DroneState.time_from_start = cur_time;
}

void Command_cb(const fast_drone_pkg::ControlCommand::ConstPtr& msg)
{
    Command_Now = *msg;
    
    // 无人机一旦接受到Land指令，则会屏蔽其他指令
    if(Command_Last.Mode == command_to_drone::Land)
    {
        Command_Now.Mode = command_to_drone::Land;
    }

    // 检测电子围栏，超过电子围栏的话就自动降落
    if(check_failsafe() == 1)
    {
        Command_Now.Mode = command_to_drone::Land;
    }
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "drone_pos_controller");
    ros::NodeHandle nh("~");

    //【订阅】无人机当前状态
    // 本话题来自根据需求自定drone_pos_estimator.cpp
    ros::Subscriber drone_state_sub = nh.subscribe<fast_drone_pkg::DroneState>("/fast_drone/drone_state", 10, drone_state_cb);

    //【订阅】指令
    // 本话题来自根据需求自定义的上层模块，比如track_circle.cpp 比如move.cpp
    ros::Subscriber Command_sub = nh.subscribe<fast_drone_pkg::ControlCommand>("/fast_drone/control_command", 10, Command_cb);

    // 【发布】日志
    // 发布log消息至ground_station.cpp
    ros::Publisher log_pub = nh.advertise<fast_drone_pkg::Topic_for_log>("/fast_drone/topic_for_log", 10);

    // 参数读取
    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
    nh.param<float>("Disarm_height", Disarm_height, 0.15);
    // nh.param<float>("Use_accel", Use_accel, 0.0);
    // nh.param<int>("Flag_printf", Flag_printf, 0.0);
    
    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -10.0);
    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 10.0);
    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -10.0);
    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 10.0);
    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -10.0);
    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 10.0);

    bool success = setUpSBusSerialPort("/dev/ttyUSB0");
    if( !success )
        throw std::runtime_error( "Unable to open UART port" );
    int len = 0;

    // 位置控制选取为50Hz，主要取决于位置状态的更新频率
    ros::Rate rate(100);

    command_to_drone _command_to_drone;
    // 位置控制类 - 串级pid
    pos_controller_cascade_PID pos_controller_cascade_pid;
    pos_controller_cascade_pid.printf_param();
    Circle_Trajectory _Circle_Trajectory;
    Eight_Trajectory _Eight_Trajectory;
    float time_trajectory = 0.0;
    float t_arm = 0.0;
    printf_param();

    int check_flag;
    // 这一步是为了程序运行前检查一下参数是否正确
    // 输入1,继续，其他，退出程序
    cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
    cin >> check_flag;

    if(check_flag != 1)
    {
        return -1;
    }

    // 先读取一些飞控的数据
    for(int i=0; i<50; i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // Set the takeoff position
    Takeoff_position[0] = _DroneState.position[0];
    Takeoff_position[1] = _DroneState.position[1];
    Takeoff_position[2] = _DroneState.position[2];

    // 初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_Now.Mode = command_to_drone::Idle;
    Command_Now.Command_ID = 0;
    Command_Now.Reference_State.Sub_mode  = command_to_drone::XYZ_POS;
    Command_Now.Reference_State.position_ref[0] = 0;
    Command_Now.Reference_State.position_ref[1] = 0;
    Command_Now.Reference_State.position_ref[2] = 0;
    Command_Now.Reference_State.velocity_ref[0] = 0;
    Command_Now.Reference_State.velocity_ref[1] = 0;
    Command_Now.Reference_State.velocity_ref[2] = 0;
    Command_Now.Reference_State.acceleration_ref[0] = 0;
    Command_Now.Reference_State.acceleration_ref[1] = 0;
    Command_Now.Reference_State.acceleration_ref[2] = 0;
    Command_Now.Reference_State.yaw_ref = 0;

    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = drone_utils::get_time_in_sec(begin_time);
    float dt = 0;

    while(ros::ok())
    {
        // 当前时间
        cur_time = drone_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.007, 0.014);
        last_time = cur_time;

        //执行回调函数
        ros::spinOnce();
        //状态机开启
        switch (Command_Now.Mode)
        {
            // 【Arm】 解锁
            case command_to_drone::Arm:
                _command_to_drone.arm_disarm(true);

            // 【Idle】 怠速旋转，此时可以切入offboard模式，但不会起飞。
            case command_to_drone::Idle:
                _command_to_drone.idle();
                break;
            // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持0
            case command_to_drone::Takeoff:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                Command_to_gs.Reference_State.Sub_mode  = command_to_drone::XYZ_POS;
                Command_to_gs.Reference_State.position_ref[0] = Takeoff_position[0];
                Command_to_gs.Reference_State.position_ref[1] = Takeoff_position[1];
                Command_to_gs.Reference_State.position_ref[2] = Takeoff_height;// + Takeoff_position[2];
                Command_to_gs.Reference_State.velocity_ref[0] = 0;
                Command_to_gs.Reference_State.velocity_ref[1] = 0;
                Command_to_gs.Reference_State.velocity_ref[2] = 0;
                Command_to_gs.Reference_State.acceleration_ref[0] = 0;
                Command_to_gs.Reference_State.acceleration_ref[1] = 0;
                Command_to_gs.Reference_State.acceleration_ref[2] = 0;
                Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad

                _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);
                
                throttle_sp[0] = _ControlOutput.Throttle[0];
                throttle_sp[1] = _ControlOutput.Throttle[1];
                throttle_sp[2] = _ControlOutput.Throttle[2];

                _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                _command_to_drone.send_attitude_setpoint(_AttitudeReference);   

                break;
            // 【Move_ENU】 ENU系移动。
            case command_to_drone::Move_ENU:
                Command_to_gs = Command_Now;

                _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                throttle_sp[0] = _ControlOutput.Throttle[0];
                throttle_sp[1] = _ControlOutput.Throttle[1];
                throttle_sp[2] = _ControlOutput.Throttle[2];

                _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                _command_to_drone.send_attitude_setpoint(_AttitudeReference);

                break;
            // Throw_Hover 抛飞。
            case command_to_drone::Throw_Hover:
                //先判断解锁条件
                if (_DroneState.position[2] > 1 && _DroneState.attitude[0] < 1 && _DroneState.attitude[1] < 1)
                {
                    _command_to_drone.arm_disarm(true);
                    if (t_arm  > 0.1)
                    {
                        Command_Now.Mode = command_to_drone::Takeoff;
                    }
                    t_arm = t_arm + dt;
                }
                
                break;
            // 【Hold】 悬停。当前位置悬停
            case command_to_drone::Hold:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                if (Command_Last.Mode != command_to_drone::Hold)
                {
                    Command_to_gs.Reference_State.Sub_mode  = command_to_drone::XYZ_POS;
                    Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0];
                    Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1];
                    Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2];
                    Command_to_gs.Reference_State.velocity_ref[0] = 0;
                    Command_to_gs.Reference_State.velocity_ref[1] = 0;
                    Command_to_gs.Reference_State.velocity_ref[2] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[0] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[1] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[2] = 0;
                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
                }
                _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                throttle_sp[0] = _ControlOutput.Throttle[0];
                throttle_sp[1] = _ControlOutput.Throttle[1];
                throttle_sp[2] = _ControlOutput.Throttle[2];

                _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                _command_to_drone.send_attitude_setpoint(_AttitudeReference);
                break;
            // 【Land】 降落。当前位置原地降落，降落后会自动上锁
            case command_to_drone::Land:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                if (Command_Last.Mode != command_to_drone::Land)
                {
                    Command_to_gs.Reference_State.Sub_mode  = command_to_drone::XYZ_POS;
                    Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0];
                    Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1];
                    Command_to_gs.Reference_State.position_ref[2] = Takeoff_position[2];
                    Command_to_gs.Reference_State.velocity_ref[0] = 0;
                    Command_to_gs.Reference_State.velocity_ref[1] = 0;
                    Command_to_gs.Reference_State.velocity_ref[2] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[0] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[1] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[2] = 0;
                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
                }

                //如果距离起飞高度小于10厘米，则直接上锁或者切换成怠速模式；
                if(abs(_DroneState.position[2] - Takeoff_position[2]) < Disarm_height)
                {
                    Command_Now.Mode = command_to_drone::Disarm;
                }else
                {
                    _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                    throttle_sp[0] = _ControlOutput.Throttle[0];
                    throttle_sp[1] = _ControlOutput.Throttle[1];
                    throttle_sp[2] = _ControlOutput.Throttle[2];

                    _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                    _command_to_drone.send_attitude_setpoint(_AttitudeReference);
                }
                break;
            // 【Disarm】 紧急上锁。直接上锁，不建议使用，危险。
            case command_to_drone::Disarm:
                t_arm = 0.0;
                _command_to_drone.arm_disarm(false);
                break;
            // Circle_Tracking 轨迹追踪控制，与上述追踪点或者追踪速度不同，此时期望输入为一段轨迹
            case command_to_drone::Circle_Tracking:
                //还没写好
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                if (Command_Last.Mode != command_to_drone::Circle_Tracking)
                {
                    time_trajectory = 0.0;
                }

                time_trajectory = time_trajectory + dt;

                Command_to_gs.Reference_State = _Circle_Trajectory.Circle_trajectory_generation(time_trajectory);

                _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                throttle_sp[0] = _ControlOutput.Throttle[0];
                throttle_sp[1] = _ControlOutput.Throttle[1];
                throttle_sp[2] = _ControlOutput.Throttle[2];

                _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                _command_to_drone.send_attitude_setpoint(_AttitudeReference);
                // Quit  悬停于最后一个目标点
                if (time_trajectory >= _Circle_Trajectory.time_total)
                {
                    Command_Now.Mode = command_to_drone::Move_ENU;
                    Command_Now.Reference_State = Command_to_gs.Reference_State;
                }
                break;
            // Eight_Tracking 轨迹追踪控制，与上述追踪点或者追踪速度不同，此时期望输入为一段8字轨迹
            case command_to_drone::Eight_Tracking:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                
                if (Command_Last.Mode != command_to_drone::Eight_Tracking)
                {
                    time_trajectory = 0.0;
                }

                time_trajectory = time_trajectory + dt;

                Command_to_gs.Reference_State = _Eight_Trajectory.Eight_trajectory_generation(time_trajectory);

                _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                throttle_sp[0] = _ControlOutput.Throttle[0];
                throttle_sp[1] = _ControlOutput.Throttle[1];
                throttle_sp[2] = _ControlOutput.Throttle[2];

                _AttitudeReference = drone_utils::ThrottleToAttitude(throttle_sp, Command_to_gs.Reference_State.yaw_ref);

                _command_to_drone.send_attitude_setpoint(_AttitudeReference);

                // Quit  悬停于最后一个目标点
                if (time_trajectory >= _Eight_Trajectory.time_total)
                {
                    Command_Now.Mode = command_to_drone::Move_ENU;
                    Command_Now.Reference_State = Command_to_gs.Reference_State;
                }
                break;
        }

        // For log
        if(cur_time == 0)
        {
            _Topic_for_log.time = -1.0;
        }
        else
        {
            _Topic_for_log.time = cur_time;
        }

        _Topic_for_log.header.stamp = ros::Time::now();
        _Topic_for_log.Drone_State = _DroneState;
        _Topic_for_log.Control_Command = Command_to_gs;
        _Topic_for_log.Attitude_Reference = _AttitudeReference;
        _Topic_for_log.Control_Output = _ControlOutput;

        log_pub.publish(_Topic_for_log);

        Command_Last = Command_Now;

        sbusPreparePacket(radioMsg,channels);
        len = write(serial_port_fd_,radioMsg,sizeof(radioMsg));
        rate.sleep();
    }
    
    return 0;
}


void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Takeoff_height: "<< Takeoff_height<<" [m] "<<endl;
    cout << "Disarm_height : "<< Disarm_height <<" [m] "<<endl;
    cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
    cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
    cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;
    
}


int check_failsafe()
{
    if (_DroneState.position[0] < geo_fence_x[0] || _DroneState.position[0] > geo_fence_x[1] ||
        _DroneState.position[1] < geo_fence_y[0] || _DroneState.position[1] > geo_fence_y[1] ||
        _DroneState.position[2] < geo_fence_z[0] || _DroneState.position[2] > geo_fence_z[1])
    {
        return 1;
        cout << "Out of the geo fence, the drone is landing... "<< endl;
    }
    else{
        return 0;
    }
}















